import sys
import os
from gui_ui import Ui_MainWindow as g
from PyQt5 import QtCore, QtGui, QtWidgets
import sys
import open3d as o3d
import numpy as np
import cal
import pyqtgraph.opengl as gl
from PyQt5.QtWidgets import QApplication, QVBoxLayout, QPushButton, QWidget, QFileDialog
import pyqtgraph as pg
pg.setConfigOption('background', 'w')


class initasd(g):
    def init(self, mainwindow):
        # super().setupUi()
        self.btn_sel.clicked.connect(self.show)
        self.p = mainwindow
        self.rec.clicked.connect(self.recCLoud)

        x = gl.GLLinePlotItem(pos=np.asarray(
            [[0, 0, 0], [0.2, 0, 0]]), color=(1, 0, 0, 1), width=0.01)
        y = gl.GLLinePlotItem(pos=np.asarray(
            [[0, 0, 0], [0, 0.2, 0]]), color=(0, 1, 0, 1), width=0.01)
        z = gl.GLLinePlotItem(pos=np.asarray(
            [[0, 0, 0], [0, 0, 0.2]]), color=(0, 0, 1, 1), width=0.01)
        self.viewer.addItem(x)
        self.viewer.addItem(y)
        self.viewer.addItem(z)

        x = gl.GLLinePlotItem(pos=np.asarray(
            [[0, 0, 0], [1.0, 0, 0]]), color=(1, 0, 0, 1), width=0.01)
        y = gl.GLLinePlotItem(pos=np.asarray(
            [[0, 0, 0], [0, 1.0, 0]]), color=(0, 1, 0, 1), width=0.01)
        z = gl.GLLinePlotItem(pos=np.asarray(
            [[0, 0, 0], [0, 0, 1.0]]), color=(0, 0, 1, 1), width=0.01)
        self.viewer_2.addItem(x)
        self.viewer_2.addItem(y)
        self.viewer_2.addItem(z)

    def show(self):
        fileName=QFileDialog.getExistingDirectory(self.p, "请选择点云：", "~")
        # fileName = "../img3"
        self.fileName = fileName
        print(fileName)
        i = 0
        import cv2
        dep = cv2.imread(f'{fileName}/depth/{i}d.png', 2)/10.0
        dep = np.array(dep, np.uint8)
        # print(dep)
        im_color = cv2.applyColorMap(dep, cv2.COLORMAP_JET)
        cv2.imwrite(f'{fileName}/depth/{i}c.png', im_color)
        scene = QtWidgets.QGraphicsScene()           # 加入 QGraphicsScene
        scene.setSceneRect(0, 0, 480, 640)           # 设定 QGraphicsScene 位置与大小
        img = QtGui.QPixmap(f'{fileName}/rgb/{i}.png')              # 加入图片
        scene.addPixmap(img)                         # 將图片加入 scene
        self.graphicsView.setScene(scene)
        scene2 = QtWidgets.QGraphicsScene()           # 加入 QGraphicsScene
        scene2.setSceneRect(0, 0, 480, 640)           # 设定 QGraphicsScene 位置与大小
        img2 = QtGui.QPixmap(f'{fileName}/depth/{i}c.png')              # 加入图片
        scene2.addPixmap(img2)                         # 將图片加入 scene
        self.graphicsView_2.setScene(scene2)

    def showcloud(self):
        pcd = o3d.io.read_point_cloud("test.ply")  # 读取点云
        np_points = np.asarray(pcd.points)  # 获取Numpy数组
        colors = np.asarray(pcd.colors)  # 获取Numpy数组
        plot = gl.GLScatterPlotItem()  # 创建显示对象
        # Ensures not to allow vertexes located behinde other vertexes to be seen.
        plot.setGLOptions('opaque')

        plot.setData(pos=np_points, color=colors,
                     size=1, pxMode=True)  # 设置显示数据
        self.viewer.addItem(plot)  # 显示点云

    def showcloud2(self):
        pcd = o3d.io.read_point_cloud("plant.pcd")  # 读取点云
        np_points = np.asarray(pcd.points)  # 获取Numpy数组
        colors = np.asarray(pcd.colors)  # 获取Numpy数组
        plot = gl.GLScatterPlotItem()  # 创建显示对象
        # Ensures not to allow vertexes located behinde other vertexes to be seen.
        plot.setGLOptions('opaque')

        plot.setData(pos=np_points, color=colors,
                     size=1, pxMode=True)  # 设置显示数据
        self.viewer_2.addItem(plot)  # 显示点云

    def recCLoud(self):
        os.system(
            f"./utils/mapper_from_images {self.fileName} ../camera.yml  0.08 ARUCO A -ref 26  100")
        os.system(f"python3 ../py/gencloud.py  {self.fileName}")
        self.showcloud()
        os.system(f"python3 ../py/moveboard.py  0.02")
        self.showcloud2()
        pcd = o3d.io.read_point_cloud("plant.pcd")
        he=cal.cal_aabb(pcd)

        import cv2
        pose = cv2.FileStorage(f'out.yaml', cv2.FileStorage_READ)
        area=pose.getNode("area").real()
        print(area)
        pose.release()


        # pose = cv2.FileStorage(f'out.yaml', cv2.FileStorage_READ)

        # # self.show(self.p, self.graphicsView)
        _translate = QtCore.QCoreApplication.translate
        self.gc_len.setText(_translate("MainWindow", "冠层长：%.2f cm"%(he[0])))
        self.gc_wid.setText(_translate("MainWindow", "冠层宽：%.2f cm"%(he[1])))
        self.area.setText(_translate("MainWindow", "投影面积：%.2f cm^2"%(area)))
        self.zh.setText(_translate("MainWindow", "株高：%.2f cm"%(he[2])))

if __name__ == "__main__":
    app = QtWidgets.QApplication(sys.argv)  # 创建一个qapplication，也就是你要开发的软件app
    mainwindow = QtWidgets.QMainWindow()  # 创建一个qmainwindow，用来装载你需要的各种组件、控件
    gui = initasd()             # ui是你创建的ui类的实例化对象
    gui.setupUi(mainwindow)         # 执行类中的setupui方法，方法的参数是第二步中创建的qmainwindow
    gui.init(mainwindow)

    mainwindow.show()            # 执行qmainwindow的show()方法，显示这个qmainwindow
    sys.exit(app.exec_())          # 使用exit()或者点击关闭按钮退出qapplication
